#include <GameEngine/Maps/square_map.h>

#include <GameEngine/Maps/map_cell.h>
#include <GameEngine/Core/Memory/memory_manager.h>

#include <math.h>


#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=====================================================================

Q_DECLARE_OPERATORS_FOR_FLAGS(SquareMap::Connectivity)

//=====================================================================


//! Constructor
/*!
 @param width The width of the square map
 @param height The height of the square map
 */
SquareMap::SquareMap(MapSize width, MapSize height) :
	cells_(nil), Map(width,height)
{
	// Allocate cell array
	MapSize sz = width * height;
	if ( sz > 0 ) cells_ = MemoryManager::instance()->allocate<Cell>(sz);
}

//! Destructor
SquareMap::~SquareMap() {
	MemoryManager::instance()->desallocate(cells_,width_*height_);
}

//! Compute the index of the cell from the location of one of its points
/*!
 @param index Receives the index of the cell, if this cells exists
 @param rhs The location of a point that belongs to the cell
 @return True if a cell containing the given point exists, false otherwise
 */
bool SquareMap::get_cell_index(CellIndex& index, const Point2d<float>& rhs) const {
	// Convert to cell coordinates and return cell
	MapIndex x = 0, y = 0;
	if ( get_map_indexes(x,y,rhs) == false ) return false;
	return get_cell_index(index,x,y);
}

//! Compute the index of the cell from the (x,y) indexes along the grid
/*!
 @param index Receives the index of the cell, if this cells exists
 @param x The x index of the cell
 @param y The y index of the cell
 @return True if a cell with given (x,y) indexes exists, false otherwise
 */
bool SquareMap::get_cell_index(CellIndex& index, MapIndex x, MapIndex y) const {
	if ( x >= width_ ) return false;
	if ( y >= height_ ) return false;
	index = get_array_index(x,y);
	return true;
}

//! Compute the map indexes of the cell from the location of one of its points
/*!
 @param x Receives the x index of the cell, if this cells exists
 @param y Receives the y index of the cell, if this cells exists
 @param rhs The location of a point that belongs to the cell
 @return True if a cell containing the given point exists, false otherwise
 */
bool SquareMap::get_map_indexes(MapIndex& x, MapIndex& y, const Point2d<float>& rhs) const {
	// Convert point to local coordinates
	Point2d<float> local = global_to_local_coordinates(rhs);
	if ( ( local.x() < 0 ) || ( local.y() < 0 ) ) return false;

	// Convert to cell coordinates and return cell
	x = (MapIndex)(local.x() * (float)width_);
	y = (MapIndex)(local.y() * (float)height_);
	if ( x >= width_ ) return false;
	if ( y >= height_ ) return false;
	return true;
}

//! Get the cell of the map at given location, if any
/*!
 @param rhs The location where a cell should be found, specified in world coordinates
 @return A pointer to the cell (may be nil if the map does not have any cell at given location)
 */
SquareMap::Cell* SquareMap::get_cell(const Point2d<float>& rhs) const {
	// Get cell index
	CellIndex idx = 0;
	if ( get_cell_index(idx,rhs) == false ) return nil;

	// Retrieve cell
	return get_cell(idx);
}

//! Accessor to one of the cells of the map
/*!
 @param x The index of the cell along the width axis
 @param y The index of the cell along the height axis
 @return A pointer to the cell (may be nil if the map does not have any cell at given index)
 */
SquareMap::Cell* SquareMap::get_cell(MapIndex x, MapIndex y) const {
	// Get cell index
	CellIndex idx = 0;
	if ( get_cell_index(idx,x,y) == false ) return nil;

	// Retrieve cell
	return get_cell(idx);
}

//! Accessor to one of the cells of the map
/*!
 @param index The index of the cell in the cell array
 @return A pointer to the cell (may be nil if the map does not have any cell at given index)
 */
SquareMap::Cell* SquareMap::get_cell(CellIndex index) const {
	if ( index >= (width_*height_) ) return nil;
	return (cells_+index);
}

//! Compute the geometry of the cell at given map coordinates, if any
/*!
 @param x The x map coordinate where a cell should be found
 @param y The y map coordinate where a cell should be found
 @return The geometry of the cell, if any
 */
std::vector< Point2d<float> > SquareMap::get_cell_geometry(MapIndex x, MapIndex y) const {
	// Rebuild cell from these indexes
	Parallelogram2d<float> cell;
	cell.v1() = map_box_.v1() / (float)width_;
	cell.v2() = map_box_.v2() / (float)height_;
	cell.origin() = Vector2d<float>(Point2d<float>(0.0f,0.0f),map_box_.origin()) + (float)x*cell.v1() + (float)y*cell.v2();

	// Put the four cell points in the result vector and return result
	int order[] = {0, 1, 3, 2};
	std::vector< Point2d<float> > res;
	for ( int k = 0; k < 4; ++k ) {
		res.push_back( cell[(Parallelogram2D::Index)order[k]] );
	}
	return res;
}

//! Compute the geometry of the cell at given location, if any
/*!
 @param rhs The location where a cell should be found, specified in world coordinates
 @return The geometry of the cell, if any
 */
std::vector< Point2d<float> > SquareMap::get_cell_geometry(const Point2d<float>& rhs) const {
	std::vector< Point2d<float> > res;

	// Get the map indexes of the cell
	MapIndex x = 0, y = 0;
	if ( get_map_indexes(x,y,rhs) == false ) return res;

	// Build corresponding hexagon
	return get_cell_geometry(x,y);
}

//=================================================================================

//! Constructor
/*!
 @param map The map that defines the space of research
 @param displacement_types The types of displacement allowed on the map
 */
SquareMapSearch::SquareMapSearch(const SquareMap& map, const Displacement::DisplacementTypes& displacement_types) :
	map_(map), displacement_types_(displacement_types) {}

//! Desctructor
SquareMapSearch::~SquareMapSearch() {}

//! Compute the list of all cells adjacent to the input index
/*!
 @param children The list of all cells adjacent to the input index
 @param node The node whose adjacent cells should be found
 @return Always return true
 */
bool SquareMapSearch::get_children(std::list<Node>& children, Node node) const {
	std::list<Node> res;

	// Get the node indexes
	SquareMap::MapIndex x = 0, y = 0;
	map_.get_axes_indexes(x, y, node);

	// Populate the list
	int x_offset[] = { 1,-1, 0, 0};
	int y_offset[] = { 0, 0, 1,-1};
	for ( int k = 0; k < 4; ++k ) {
		// If cell does not exist, do nothing
		SquareMap::Cell* cell = map_.get_cell(x+x_offset[k], y+y_offset[k]);
		if ( cell == nil ) continue;

		// If the cell is not walkable, do not add it
		if ( map_.terrains().get_terrain(cell->terrain()).speed(displacement_types_) <= 0 ) continue;

		// Add the cell to the list
		children.push_back( map_.get_array_index(x+x_offset[k], y+y_offset[k]) );
	}

	// Return result
	return true;
}

//! Compute the cost to go from the parent to the current node
/*!
 @param parent The parent of the current node
 @param node The current node
 @return The cost to go from the parent to the current node
 */
SquareMapSearch::Cost SquareMapSearch::cost(Node parent, Node node) const {
	// Get the corresponding cells
	SquareMap::Cell* parent_cell = map_.get_cell(parent);
	SquareMap::Cell* node_cell = map_.get_cell(node);

	// Get the descriptions associated to the nodes
	const TerrainDescriptor& parent_desc = map_.terrains().get_terrain( parent_cell ? parent_cell->terrain() : TerrainDescriptor::InvalidTerrain );
	const TerrainDescriptor& node_desc = map_.terrains().get_terrain( node_cell ? node_cell->terrain() : TerrainDescriptor::InvalidTerrain );

	// Get the costs associated to the nodes
	Cost parent_cost = parent_desc.speed(displacement_types_);
	Cost node_cost = node_desc.speed(displacement_types_);

	// Compute the cost as the average of both costs
	return ( map_.map_box_.v1().length() / (float)map_.width_ + map_.map_box_.v2().length() / (float)map_.height_ ) / ( parent_cost + node_cost );
}

//=================================================================================

//! Constructor
/*!
 @param map The map that defines the space of research
 */
SquareMapSearchHeuristic::SquareMapSearchHeuristic(const SquareMap& map) : map_(map) {}

//! Desctructor
SquareMapSearchHeuristic::~SquareMapSearchHeuristic() {}

//! Compute the cost to go from the parent to the current node
/*!
 *  Cost is estimated as the euclidean distance between the nodes
 *
 @param start The current node
 @param end The node to get to
 @return The estimated cost to go from the current to the end node
 @note This function implicitly assumes that the speed of displacement is 1 all over the map
 */
SquareMapSearch::Cost SquareMapSearchHeuristic::cost(SquareMapSearch::Node start, SquareMapSearch::Node end) const {
	// Get the axes indexes
	SquareMap::MapIndex start_x = 0, start_y = 0, end_x = 0, end_y = 0;
	map_.get_axes_indexes(start_x, start_y, start);
	map_.get_axes_indexes(end_x, end_y, end);

	// Get the distance between the cells
	SquareMapSearch::Cost sq_dist = (start_x-end_x)*(start_x-end_x) + (start_y-end_y)*(start_y-end_y);

	// Compute the cost as the average of both costs
	return sqrt(sq_dist) * ( map_.map_box_.v1().length() / (float)map_.width_ + map_.map_box_.v2().length() / (float)map_.height_ );
}



#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif
